#ifndef __cube_robot_main_h
#define __cube_robot_main_h

// 每圈200步（360/1.8），16细分，每一分钟60秒
#define RPM (200.0f * 16.0f / 60.0f)
#define FLASH_CONFIG_VERSION 0x01
typedef union GCONFIG_STRUCT
{
  uint8_t date[256];
  struct
  {
    float SPEED_HOME;
    float SPEED_LOW;
    float ACCEL_HOME;
    float V_START_FINGER;
    float V_MAX_FINGER;
    float A_MAX_FINGER;
    float V_START_ARM;
    float V_MAX_ARM;
    float A_MAX_ARM;
    float V_START_ARM_L;
    float V_MAX_ARM_L;
    float A_MAX_ARM_L;
    int DELAY_US_AFTER_FINGER_LOCK;
    float SPEED_FACTOR_FULL;
    float SPEED_FACTOR_SLOW;
    // ARM_OFFSET 零点距离光电开关的角度，720 * (1.8 / 16) = 81度
    int ARM_OFFSET;

    // FINGER_OFFSET，每个微步代表8*1.5*2*pi/(200*16) = 3*pi/400 = 0.02356mm
    // 利用爪子的型变弹力，可以单臂夹紧魔方，不滑落的状态
    // 如果夹不紧，还原过程中容易松动，导致魔方滑落，改小
    // 如果夹的过紧，导致魔方滑落，容易丢步，改大
    int FINGER_OFFSET_LOCK;
    // 利用爪子的型变弹力，可以拧动魔方的状态
    int FINGER_OFFSET_SPIN;
    // 回零点后，需要手指移动到的位置，比较松，方便放置魔方
    int FINGER_OFFSET_INIT;
    // 允许的最远位置
    int FINGER_OFFSET_MAX;
    // IRUN IHOLD取值范围0-31
    // Irms = (CS + 1)/32*180/(100+20)/sqrt(2)
    // Ip   = (CS + 1)/32*180/(100+20)
    // 电机厂商标称的最大电流,通常为峰值
    int IRUN;
    int IHOLD;
    // SGTHRS 无限位归零阈值，数值越大，越容易触发
    // 如果回零时有较大响声，改大
    // 如果回零时提前停止，说明灵敏度过高，改小
    int SGTHRS;
    int DEBUG_MODE;
  };
} g_config;

/*
方向定义
FINGER_OFFSET_ZERO
DIR=0
  |
  |
  |
DIR=1
FINGER_OFFSET_MAX
*/

// 电机0 2
#define LEFT 0
// 电机1 3
#define RIGHT 1

#define CCW 0
#define CW 1

extern g_config cfg;
extern int exit_debug_mode;

void zero_point(void);
void move_finger_to_default(void);
void move_finger_to_init(void);
void move_finger_to_max(void);
void move_finger_to_lock(void);
void twist_cube_180(int lr);
void twist_cube_90(int lr, int cw_ccw, int skip_back_step);
void flip_cube_180(int lr);
void flip_cube_90(int lr, int cw_ccw, int skip_back_step);
void run_demo(void);
void tmc2209_config(uint8_t addr);
/*
flip_cube_90(LEFT, CCW, 0);
flip_cube_90(RIGHT, CCW, 0);
flip_cube_90(LEFT, CW, 0);
flip_cube_90(RIGHT, CW, 0);
twist_cube_180(LEFT);
twist_cube_180(RIGHT);
twist_cube_90(LEFT, CCW, 0);
twist_cube_90(LEFT, CW, 0);
twist_cube_90(RIGHT, CCW, 0);
twist_cube_90(RIGHT, CW, 0);
flip_cube_180(LEFT);
flip_cube_180(RIGHT);
*/


#endif